#!/usr/bin/env python
import rclpy
from rclpy.qos import QoSProfile, qos_profile_system_default
from geometry_msgs.msg import TwistStamped
from agv_srvs.srv import SetOperationMode  
import sys
import select
import termios
import tty
from time import time
settings = termios.tcgetattr(sys.stdin)
msg = """
Reading from the keyboard and Publishing to TwistStamped!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
   U    I    O
   J    K    L
   M    <    >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
1 : switch to manual mode
2 : switch to halfAuto mode
3 : switch to auto mode
"""
moveBindings = {
    'i': (1, 0, 0, 0),
    'o': (1, 0, 0, -1),
    'j': (0, 0, 0, 1),
    'l': (0, 0, 0, -1),
    'u': (1, 0, 0, 1),
    ',': (-1, 0, 0, 0),
    '.': (-1, 0, 0, 1),
    'm': (-1, 0, 0, -1),
    'O': (1, -1, 0, 0),
    'I': (1, 0, 0, 0),
    'J': (0, 1, 0, 0),
    'L': (0, -1, 0, 0),
    'U': (1, 1, 0, 0),
    '<': (-1, 0, 0, 0),
    '>': (-1, -1, 0, 0),
    'M': (-1, 1, 0, 0),
    't': (0, 0, 1, 0),
    'b': (0, 0, -1, 0),
}
speedBindings = {
    'q': (1.1, 1.1),
    'z': (.9, .9),
    'w': (1.1, 1),
    'x': (.9, 1),
    'e': (1, 1.1),
    'c': (1, .9),
}
def getKey():
    tty.setraw(sys.stdin.fileno())
    select.select([sys.stdin], [], [], 0)
    key = sys.stdin.read(1)
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key
def vels(speed, turn):
    return "currently:\tspeed %s\tturn %s " % (speed, turn)
def main(args=None):
    rclpy.init(args=args)
    node = rclpy.create_node('teleop_twist_keyboard')
    pub = node.create_publisher(TwistStamped, 'cmd_vel', qos_profile=qos_profile_system_default)
    # 创建服务客户端
    client = node.create_client(SetOperationMode, 'operation_mode')  
    
    speed = 0.5
    turn = 1.0
    x = 0
    y = 0
    z = 0
    th = 0
    status = 0
    try:
        print(msg)
        print(vels(speed, turn))
        while True:
            key = getKey()
            if key in moveBindings.keys():
                x = moveBindings[key][0]
                y = moveBindings[key][1]
                z = moveBindings[key][2]
                th = moveBindings[key][3]
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]
                turn = turn * speedBindings[key][1]
                print(vels(speed, turn))
                if (status == 14):
                    print(msg)
                status = (status + 1) % 15
            elif key == '1':
                # 切换到 manual 模式
                request = SetOperationMode.Request()
                request.mode = "manual"
                request.submode = "local"
                request.sender = 1
                future = client.call_async(request)
                rclpy.spin_until_future_complete(node, future)
                if future.result() is not None:
                    print("Switched to manual mode")
                else:
                    print("Failed to switch to manual mode")
            elif key == '2':
                # 切换到 halfAuto 模式
                request = SetOperationMode.Request()
                request.mode = "auto"
                request.submode = "local"
                request.sender = 1
                future = client.call_async(request)
                rclpy.spin_until_future_complete(node, future)
                if future.result() is not None:
                    print("Switched to local Auto mode")
                else:
                    print("Failed to switch to local Auto mode")
            elif key == '3':
                # 切换到 auto 模式
                request = SetOperationMode.Request()
                request.mode = "auto"
                request.submode = "master"
                request.sender = 1
                future = client.call_async(request)
                rclpy.spin_until_future_complete(node, future)
                if future.result() is not None:
                    print("Switched to Master auto mode")
                else:
                    print("Failed to switch to Master auto mode")
            else:
                x = 0
                y = 0
                z = 0
                th = 0
                if (key == '\x03'):
                    break
            twist_stamped = TwistStamped()
            twist_stamped.header.stamp.sec = int(time())
            twist_stamped.header.stamp.nanosec = int((time() - int(time())) * 1e9)
            twist_stamped.twist.linear.x = x * speed
            twist_stamped.twist.linear.y = y * speed
            twist_stamped.twist.linear.z = z * speed
            twist_stamped.twist.angular.x = 0.0
            twist_stamped.twist.angular.y = 0.0
            twist_stamped.twist.angular.z = th * turn
            pub.publish(twist_stamped)
    except Exception as e:
        print(e)
    finally:
        twist_stamped = TwistStamped()
        twist_stamped.twist.linear.x = 0.0
        twist_stamped.twist.linear.y = 0.0
        twist_stamped.twist.linear.z = 0.0
        twist_stamped.twist.angular.x = 0.0
        twist_stamped.twist.angular.y = 0.0
        twist_stamped.twist.angular.z = 0.0
        pub.publish(twist_stamped)
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
if __name__ == '__main__':
    main()
